#include <stdio.h>
#include <string.h>
#include <strings.h>
#include <stdlib.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_watchdog.h"
#include "iot_gpio.h"
#include "hi_time.h"

#include "genki_wifi_sta.h"
#include "lwip/sockets.h"

// 自定义
#include "Robot.h"


// wifi
#define sta_ssid        "B-LINK_8AF4E2"
#define sta_pwd         "123456+++abc"
#define sta_hostname    "Puppy"

/* -------------------- 函数声明 -------------------- */
static void Blink(int blink_num);


//float Angle[3]      = {90,135,45};
//uint8_t legNum      = 1;

//WIFI_STA+UDP线程
static void wifi_task(void) {
    wifi_sta_connect(sta_ssid, sta_pwd, sta_hostname );

    // udp  绑定8080端口
    // udp create   创建UDP
    int sock_fd;
    int ret;
    sock_fd = socket(AF_INET, SOCK_DGRAM, 0);

    if (sock_fd < 0) {
        perror("sock_fd create error\r\n");
        return;
    }

    // config receive addr
    struct sockaddr_in recvfrom_addr;
    socklen_t recvfrom_addr_len = sizeof(recvfrom_addr);

    memset((void *) &recvfrom_addr, 0, recvfrom_addr_len);
    recvfrom_addr.sin_family = AF_INET;
    recvfrom_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    recvfrom_addr.sin_port = htons(8080);               //绑定端口

    // bind receive addr        //绑定
    // bind
    ret = bind(sock_fd, (struct sockaddr *) &recvfrom_addr, recvfrom_addr_len);
    if (ret == -1) {
        perror("bind error\r\n");
        return;
    }

    char recv_buf[1024];
    int recv_len;
    //接收消息
    while (1) {
        struct sockaddr_in sender_addr;
        int sender_addr_len;

        recv_len = recvfrom(sock_fd,
                            recv_buf,
                            sizeof(recv_buf),
                            0,
                            (struct sockaddr *) &sender_addr,
                            sender_addr_len);

        if (recv_len <= 0)
            continue;

        char recv_data[recv_len];
        memcpy(recv_data, recv_buf, recv_len);


        Blink(1);

//        legNum = FK_LegServoDebug(Angle,recv_data[0],legNum);

//        float point[3] = {-19,25.400,75.66};
//        LegPointDebug(point,recv_data[0],1,1);  //单腿调试

        switch (recv_data[0]) {
            case '0':Robot_FK_ResetPose();break;
            case '1':Robot_FK_GetDown();break;
            case '2':Robot_IK_ResetPose();break;
            case '3':Robot_Action_0();break;
            
            case '4':Trot(10,1);
                break;
            case '5':Trot(10,0);
                break;
            case '6':TrotRL(10,1);
                break;
            case '7':TrotRL(10,0);
                break;
            case '8':TrotTurn(10,1);
                break;
            case '9':TrotTurn(10,0);
                break;
            case 'a':
                Robot_PowerOFF();
                Blink(3);
                IoTGpioSetOutputVal(6,1);
                break;
        }

//        printf("len: %d data: %s\r\n", recv_len, recv_data);
    }
}



static void start(void) {
    printf("\n");
    /* ------------------ STA线程 ------------------ */
    osThreadAttr_t attr;

    attr.name       = "wifi_sta";
    attr.attr_bits  = 0U;
    attr.cb_mem     = NULL;
    attr.cb_size    = 0U;
    attr.stack_mem  = NULL;
    attr.stack_size = 1024 * 4;
    attr.priority   = 25;

    if (osThreadNew((osThreadFunc_t)wifi_task, NULL, &attr) == NULL){
        printf("Create WIFI STA task Failed!\r\n");
    }else{
        printf("Create WIFI STA task Success!\r\n");
    }
    /* ------------------------------------------------- */

    /* 初始化 */
    Robot_init();
    IoTGpioInit(IOT_IO_NAME_6);
    IoTIoSetFunc(IOT_IO_NAME_6,IOT_IO_FUNC_6_GPIO);
    IoTGpioSetDir(IOT_IO_NAME_6,IOT_GPIO_DIR_OUT);
    Blink(5);

    while(1){
        IoTWatchDogKick();  //嘿喂狗
    }
}

static void Blink(int blink_num){

    for(uint8_t i = 0;i < blink_num;i ++){
        IoTGpioSetOutputVal(6,0);
        hi_udelay(50*1000);
        IoTGpioSetOutputVal(6,1);
        hi_udelay(50*1000);
        continue;
    }
    IoTGpioSetOutputVal(6,0);
}

APP_FEATURE_INIT(start);